#include "rclcpp/rclcpp.hpp"
#include <string>
#include <vector>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "rclcpp_action/rclcpp_action.hpp"

#include "cv_bridge/cv_bridge.h"
#include <geometry_msgs/msg/pose2_d.hpp>
#include <geometry_msgs/msg/polygon.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/point32.hpp>
#include "nav_msgs/msg/occupancy_grid.hpp"
#include <ipa_building_msgs/action/room_exploration.hpp>
#include <ipa_room_exploration/timer.h>
#include <Eigen/Dense>

using ClientT=ipa_building_msgs::action::RoomExploration;
enum class ActionStatus
    {
    UNKNOWN = 0,
    PROCESSING = 1,
    FAILED = 2,
    SUCCEEDED = 3
    };

ActionStatus current_goal_status_= ActionStatus::UNKNOWN;

std::shared_ptr<ipa_building_msgs::action::RoomExploration::Result> action_result=nullptr;
// overload of << operator for geometry_msgs::msg::Pose2D to wanted format
std::ostream& operator<<(std::ostream& os, const geometry_msgs::msg::Pose2D& obj)
{
	std::stringstream ss;
	ss <<  "[" << obj.x << ", " << obj.y << ", " << obj.theta << "]";
	os << ss.rdbuf();
	return os;
}

void resultCallback(const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result)
{
	
	switch (result.code) {
		case rclcpp_action::ResultCode::SUCCEEDED:
			current_goal_status_ = ActionStatus::SUCCEEDED;
			std::cout<< "resultCallback current_goal_status_= " << (int)current_goal_status_<< std::endl;
			action_result=result.result;
			break;
		case rclcpp_action::ResultCode::ABORTED:
			current_goal_status_ = ActionStatus::FAILED;
			std::cout<< "resultCallback current_goal_status_= " << (int)current_goal_status_<< std::endl;
			break;
		case rclcpp_action::ResultCode::CANCELED:
			current_goal_status_ = ActionStatus::FAILED;
			std::cout<< "resultCallback current_goal_status_= " << (int)current_goal_status_<< std::endl;
			break;
		default:
			current_goal_status_ = ActionStatus::UNKNOWN;
			std::cout<< "resultCallback current_goal_status_= " << (int)current_goal_status_<< std::endl;
			break;
	}
}

void goalResponseCallback(const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & future)
{
	auto goal_handle = future.get();
	if (!goal_handle) 
	{
		current_goal_status_ = ActionStatus::FAILED;
		std::cout<< "action client failed to send goal to server."<< std::endl;
		return;
	}
	std::cout<< "action client success to send goal to server."<< std::endl;
}


int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node_handle = rclcpp::Node::make_shared("room_exploration_client");
	rclcpp_action::Client<ClientT>::SharedPtr room_exploration_client_;
	room_exploration_client_= rclcpp_action::create_client<ClientT>(node_handle, "room_exploration_server");
	auto send_goal_options = rclcpp_action::Client<ClientT>::SendGoalOptions();
	send_goal_options.result_callback = resultCallback;
    send_goal_options.goal_response_callback = goalResponseCallback;


	// read params
	bool use_test_maps;
	use_test_maps=node_handle->declare_parameter<bool>("use_test_maps",true);
	double resolution;
	resolution=node_handle->declare_parameter<double>("resolution",0.05);
	std::vector<double> origin (3,0);
	origin[0]= -5.75;
	origin[1]= -5.71;
	origin=node_handle->declare_parameter<std::vector<double>>("origin",origin);
	double robot_radius;
	robot_radius=node_handle->declare_parameter<double>("robot_radius",0.15);
	double coverage_radius;
	coverage_radius=node_handle->declare_parameter<double>("coverage_radius",0.15);
	std::vector<double> start_pos = {0, 0, 0};
	start_pos=node_handle->declare_parameter<std::vector<double>>("starting_position",start_pos);
	if (start_pos.size() != 3)
	{
		printf("starting_position must contain 3 values");
		return -1;
	}

	std::string image_path;
		// read in test map
	const std::string test_map_path =
	        "/home/ubuntu/ros2_ws/src/Sweeping_robot/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/";
	image_path = test_map_path + "lab_ipa_gt_segmentation.png";  //  map2.pgm  lab_ipa.png lab_ipa_gt_segmentation.png
	std::cout << "image_path: " <<image_path<< std::endl;

	cv::Mat map_flipped = cv::imread(image_path, 0);
	cv::Mat map;
	cv::flip(map_flipped, map, 0);
	//make non-white pixels black
	for (int y = 0; y < map.rows; y++)
	{
		for (int x = 0; x < map.cols; x++)
		{
			//find not reachable regions and make them black
			if (map.at<unsigned char>(y, x) < 250)
			{
				map.at<unsigned char>(y, x) = 0;
			}
			//else make it white
			else
			{
				map.at<unsigned char>(y, x) = 255;
			}
		}
	}
	std::cout << "map-size: " << map.rows << "x" << map.cols << std::endl;
	/* cv::namedWindow("map",0);
	cv::resizeWindow("map", 1000, 1000);
	cv::imshow("map", map);
	cv::waitKey(0); */
//	const std::string topic = "/move_base/global_costmap/costmap";
//	nav_msgs::OccupancyGrid grid;
//	grid = *(ros::topic::waitForMessage<nav_msgs::OccupancyGrid>(topic, nh));
//	printf("got grid");
//	std::vector<signed char>& dats = grid.data;
//	std::cout << dats.size() << std::endl;
//	cv::Mat test_map = cv::Mat(grid.info.height, grid.info.width, map.type());
//	for(size_t v = 0; v < test_map.rows; ++v)
//		for(size_t u = 0; u < test_map.cols; ++u)
//			test_map.at<uchar>(v,u) = (uchar)(2.55*(double)dats[v*grid.info.width+u]);
//	cv::imshow("testtt", test_map);
//	cv::waitKey();

	// wait for the action server to start

	
//DynamicReconfigureClient drc_exp(nh, "room_exploration_server/set_parameters", "room_exploration_server/parameter_updates");
//drc_exp.setConfig("room_exploration_algorithm", 8);
//drc_exp.setConfig("execute_path", false);
//	drc_exp.setConfig("path_eps", 3);
//	drc_exp.setConfig("grid_line_length", 15);
//	drc_exp.setConfig("path_eps", 10);
//	drc_exp.setConfig("cell_size", 10);
//	drc_exp.setConfig("plan_for_footprint", true);
//	drc_exp.setConfig("goal_eps", 0.0);
//	drc_exp.setConfig("delta_theta", 0.005);

//	cv::Point2f src_center(map.cols/2.0F, map.rows/2.0F);
//	cv::Mat rot_mat = getRotationMatrix2D(src_center, 180, 1.0);
//	cv::Mat dst;
//	cv::warpAffine(map, dst, rot_mat, map.size());
//	cv::flip(dst, map, 1);
//	cv::imshow("map", map);
//	cv::waitKey();

	sensor_msgs::msg::Image labeling;
	cv_bridge::CvImage cv_image;
//	cv_image.header.stamp = ros::Time::now();
	cv_image.encoding = "mono8";
	cv_image.image = map;
	cv_image.toImageMsg(labeling);

	geometry_msgs::msg::Pose map_origin;
	map_origin.position.x = origin[0];
	map_origin.position.y = origin[1];
	map_origin.position.z = origin[2];

	geometry_msgs::msg::Pose2D starting_position;
	starting_position.x = start_pos[0];
	starting_position.y = start_pos[1];
	starting_position.theta = start_pos[2];
	int planning_mode=1;
	std::vector<geometry_msgs::msg::Point32> fov_points(4);
	/* planning_mode = 2;	// viewpoint planning
	fov_points[0].x = 0.04035;		// this field of view represents the off-center iMop floor wiping device
	fov_points[0].y = -0.136;
	fov_points[1].x = 0.04035;
	fov_points[1].y = 0.364;
	fov_points[2].x = 0.54035;		// todo: this definition is mirrored on x (y-coordinates are inverted) to work properly --> check why, make it work the intuitive way
	fov_points[2].y = 0.364;
	fov_points[3].x = 0.54035;
	fov_points[3].y = -0.136; */

//  planning_mode = 2;	// viewpoint planning
//	fov_points[0].x = 0.15;		// this field of view fits a Asus Xtion sensor mounted at 0.63m height (camera center) pointing downwards to the ground in a respective angle
//	fov_points[0].y = 0.35;
//	fov_points[1].x = 0.15;
//	fov_points[1].y = -0.35;
//	fov_points[2].x = 1.15;
//	fov_points[2].y = -0.65;
//	fov_points[3].x = 1.15;
//	fov_points[3].y = 0.65;

	planning_mode = 1;	// footprint planning
	fov_points[0].x = -0.15;		// this is the working area of a vacuum cleaner with 60 cm width
	fov_points[0].y = 0.15;
	fov_points[1].x = -0.15;
	fov_points[1].y = -0.15;
	fov_points[2].x = 0.15;
	fov_points[2].y = -0.15;
	fov_points[3].x = 0.15;
	fov_points[3].y = 0.15;
	geometry_msgs::msg::Point32 fov_origin;
	fov_origin.x = 0.;
	fov_origin.y = 0.;

	ipa_building_msgs::action::RoomExploration_Goal goal;
	goal.input_map = labeling;
	goal.map_resolution = resolution;
	goal.map_origin = map_origin;
	goal.robot_radius = robot_radius; // turtlebot, used for sim 0.177, 0.4
	goal.coverage_radius = coverage_radius;
	goal.field_of_view = fov_points;
	goal.field_of_view_origin = fov_origin;
	goal.starting_position = starting_position;
	goal.planning_mode = planning_mode;
	while(rclcpp::ok() && !room_exploration_client_->wait_for_action_server(std::chrono::seconds(5))) 
	{
     	std::cout<<"Action server not available after waiting, wait 5s..."<< std::endl;
  	}
	std::cout<<"Action server started, sending goal."<< std::endl;
	room_exploration_client_->async_send_goal(goal, send_goal_options);
    current_goal_status_ = ActionStatus::PROCESSING;
	rclcpp::Rate r(5);
	while(rclcpp::ok())
	{
		std::cout<< "current_goal_status_= " << (int)current_goal_status_<< std::endl;
		if(current_goal_status_ == ActionStatus::SUCCEEDED)
		{
			std::cout << "imshow result "<< std::endl;
			std::cout << "Got a path with "<<  (int)(action_result->coverage_path.size()) << " nodes" << std::endl;
			// display path
			const double inverse_map_resolution = 1./goal.map_resolution;
			cv::Mat path_map = map.clone();
			for (size_t point=0; point<action_result->coverage_path.size(); ++point)
			{
				const cv::Point point1((action_result->coverage_path[point].x-map_origin.position.x)*inverse_map_resolution, (action_result->coverage_path[point].y-map_origin.position.y)*inverse_map_resolution);
				cv::circle(path_map, point1, 2, cv::Scalar(128), cv::FILLED);
				if (point > 0)
				{
					const cv::Point point2((action_result->coverage_path[point-1].x-map_origin.position.x)*inverse_map_resolution, (action_result->coverage_path[point-1].y-map_origin.position.y)*inverse_map_resolution);
					cv::line(path_map, point1, point2, cv::Scalar(128), 1);
				}
				std::cout << "coverage_path[" << point << "]: x=" << action_result->coverage_path[point].x << ", y=" << action_result->coverage_path[point].y << ", theta=" << action_result->coverage_path[point].theta << std::endl;
			}
			cv::namedWindow("path",0);
			cv::resizeWindow("path", 1000, 1000);
			cv::imshow("path", path_map);
			cv::waitKey(0);
			break;
		}
		else if(current_goal_status_ == ActionStatus::FAILED)
		{
			std::cout<<"break :  current_goal_status_ == ActionStatus::FAILED"<< std::endl;
			break;
		}
		r.sleep();
		rclcpp::spin_some(node_handle);
	}
	std::cout<<"return"<< std::endl; 
	return 0;
}
